#pragma once

#include "vio/vio_common.h"

#include "vio/front_end/feature.h"
#include "vio/front_end/map.h"
#include "vio/front_end/patch_matcher.h"
#include "vio/front_end/point.h"
#include "vio/tools/ring_buffer.h"
#include "vio/tools/timer.h"

namespace vio {

// Base class for various VO pipelines. manages the map and the state machine
class FrameHandlerBase : boost::noncopyable {
 public:
  enum Stage {
    STAGE_PAUSED,
    STAGE_FIRST_FRAME,
    STAGE_SECOND_FRAME,
    STAGE_DEFAULT_FRAME,
    STAGE_RELOCALIZATION
  };
  enum TrackingQuality { TRACKING_INSUFFICIENT, TRACKING_BAD, TRACKING_GOOD };
  enum UpdateResult { RESULT_NO_KEYFRAME, RESULT_IS_KEYFRAME, RESULT_FAILURE };

  FrameHandlerBase();

  // get the current map
  const Map& GetMap() const {
    return map_;
  }

  // will reset the map as soon as the current frame is finished processing.
  void Reset() {
    set_reset_ = true;
  }

  // start processing
  void Start() {
    set_start_ = true;
  }

  // get the current stage of the agorithm
  Stage GetStage() const {
    return stage_;
  }

  // get tracking quality
  TrackingQuality GetTrackingQuality() const {
    return tracking_quality_;
  }

  // get the processing time of the previous iteration
  double LastProcessingTime() const {
    return timer_.getTime();
  }

  // get the number of feature observations of the last frame.
  size_t LastNumObservations() const {
    return num_obs_last_;
  }

 protected:
  Stage stage_;     // current stage of the algorithm
  bool set_reset_;  // flag that the user can set. will result the system before
                    // the next iteration
  bool set_start_;  // flag the user can set to start the system when the next
                    // image is received.
  Map map_;         // map of keyframes created by the slam system
  utils::Timer timer_;  // stopwatch to measure time to process frame.
  utils::RingBuffer<double>
      acc_frame_timings_;  // total processing time of the last 10 frames, used
                           // to give some user feedback on the performance.
  utils::RingBuffer<size_t>
      acc_num_obs_;  // number of observed features of the last 10 frames, used
                     // to give some user feedback on the tracking performance.
  size_t num_obs_last_;  // number of observations in the previous frame.
  TrackingQuality
      tracking_quality_;  // an estimate of the tracking quality based on the
                          // number of tracked features.
  // before a frame is processed, this function is called.
  bool StartFrameProcessingCommon(const double timestamp);

  // when a frame is finished processing, this function is called.
  int FinishFrameProcessingCommon(const size_t update_id,
                                  const UpdateResult dropout,
                                  const size_t num_observations);

  // reset the map and frame handler to start from scratch.
  void ResetCommon();

  // reset the frame handler. implement in derived class.
  virtual void ResetAll() {
    ResetCommon();
  }

  // set the tracking quality based on the number of trackec features.
  virtual void SetTrackingQuality(const size_t num_observations);

  // optimize some of the observed 3D points
  virtual void OptimizeStructure(Frame::Ptr frame,
                                 size_t max_n_pts,
                                 int max_iter);
};
}  // namespace vio
